#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/module.h>
#include <uORB/uORB.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <px4_platform_common/posix.h>
#include <stdio.h>
#include <unistd.h>

extern "C" __EXPORT int test_aux4_main(int argc, char *argv[]);

int test_aux4_main(int argc, char *argv[])
{
    int manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
    px4_pollfd_struct_t fds[] = {
        { .fd = manual_control_sub, .events = POLLIN }
    };

    printf("=== AUX4 Test Program ===\n");
    printf("Waiting for aux4 values from QGC...\n");
    printf("Move your joystick channel 4 to see aux4 values change\n\n");

    while (true) {
        int poll_ret = px4_poll(fds, 1, 1000); // 1s timeout

        if (poll_ret > 0 && (fds[0].revents & POLLIN)) {
            struct manual_control_setpoint_s manual_control{};
            orb_copy(ORB_ID(manual_control_setpoint), manual_control_sub, &manual_control);

            // 只显示aux4的值
            printf("AUX4 Value: %.3f", (double)manual_control.aux4);

            // 添加一些可视化指示
            if (manual_control.aux4 > 0.5f) {
                printf(" [HIGH]");
            } else if (manual_control.aux4 < -0.5f) {
                printf(" [LOW]");
            } else {
                printf(" [MID]");
            }

            printf("\n");
        }
    }

    orb_unsubscribe(manual_control_sub);
    return 0;
}
